﻿// ActionScript file
//--------------------------------------------------------------
//
// Qizhi Zhang 
// ezhung@gmail.com
// http://www.moorwind.com
//
//--------------------------------------------------------------
package com.moorwind.ai.physics
{
    import com.moorwind.ai.math.MathConst;
    import com.moorwind.ai.math.MathUtils;
    import com.moorwind.ai.math.Vector;
    
    import flash.display.Sprite;

    public class RigidBody2D extends Sprite implements IRigidBody2D
    {
        //////////////////////////////////////////////////////////////////////////////
        //Public properties
        
        /** total mass (constant) */
        public var fMass:Number = 0;
        
        /** mass moment of inertia in body coordinates (constant) */
        public var fInertia:Number = 0;
        
        /** inverse of mass moment of inertia (constant) */
        public var fInertiaInverse:Number = 0;
        
        /** position in earth coordinates */
        public var vPosition:Vector = new Vector();
        
        /** velocity in earth coordinates */
        public var vVelocity:Vector = new Vector();
        
        /** velocity in body coordinates */
        public var vVelocityBody:Vector = new Vector();
        
        /** angular velocity in body coordinates */
        public var vAngularVelocity:Vector = new Vector();
        
        /** speed (magnitude of the velocity) */
        public var fSpeed:Number = 0;
                
        /** orientation */
        public var fOrientation:Number = 0;
        
        /** total force on body */
        public var vForces:Vector = new Vector();
        
        /** total moment (torque) on body (2D: about z-axis only) */
        public var vMoment:Vector = new Vector();
        
        /** Magnitude of the thrust force */
        public var thrustForce:Number = 0;
        
        /** bow thruster forces */
        public var PThrust:Vector = new Vector(), SThrust:Vector = new Vector();
        
        /** bounding dimensions */
        public var fWidth:Number = 0;
        
        /** bounding dimensions */
        public var fHeight:Number = 0;
        
        /** bounding dimensions */
        public var fLength:Number = 0;        
        
        public var CD:Vector = new Vector();                
        public var CT:Vector = new Vector();        
        public var CPT:Vector = new Vector();        
        public var CST:Vector = new Vector();
        
        public var projectedArea:Number = 0;
        
        /** other applied force */
        public var Fa:Vector = new Vector();
        
        /** location of other applied force */
        public var Pa:Vector = new Vector();
        
        //////////////////////////////////////////////////////////////////////////////
        //Private constant variables
        
        private const _LINEARDRAGCOEFFICIENT:Number = 0.3;
        private const _ANGULARDRAGCOEFFICIENT:Number = 5000.0;
        private const _STEERINGFORCE:Number = 6.0;
        private const _DTHRUST:Number = 0.01;
        private const _MINTHRUST:Number = 0.0;
        private const _MAXTHRUST:Number = 80.0 / 2;
        private const _THRUSTFORCE:Number = 20.0 / 2;
        
        //////////////////////////////////////////////////////////////////////////////
        //Constructor
        public function RigidBody2D()
        {
            super();
        }
        
        //////////////////////////////////////////////////////////////////////////////
        //methods
        
        /**
         * Calculate forces and moments
         */
        public function calcLoad():void
        {
            var Fb:Vector = new Vector();
            var Mb:Vector = new Vector();
            var Thrust:Vector = new Vector();
            
            //--------------------------------------------------------------
            //
            // reset forces and moments
            //
            //--------------------------------------------------------------
            
            //vForces.z is always zero in 2D
            this.vForces = new Vector();
            
            //vMoment.x and vMoment.y are always zero in 2D
            this.vMoment = new Vector();
            
            Fb = new Vector();
            Mb = new Vector();
                        
            // Define the thrust vector, which acts through the craft's CG
            Thrust = new Vector(0,1,0);
            Thrust = Vector.multiplication(this.thrustForce, Thrust);
            
            var vLocalVelocity:Vector = new Vector();
            var fLocalSpeed:Number;
            var vDragVector:Vector = new Vector();
            var tmp:Number;
            var vResultant:Vector = new Vector();
            var vtmp:Vector = new Vector();
            
            //--------------------------------------------------------------
            //
            // Calculate the aerodynamic drag force
            //
            //--------------------------------------------------------------
            
            // Calculate local velocity:
            // The local velocity includes the velocity due to linear motion of the craft, 
            // plus the velocity at each element due to the rotation of the craft.
            vtmp = Vector.exterior(this.vAngularVelocity, this.CD); // rotational part
            vLocalVelocity = Vector.addition(this.vVelocityBody, vtmp);
            
            // Calculate local air speed
            fLocalSpeed = this.vVelocityBody.length;
            
            // Find the direction in which drag will act.
            // Drag always acts inline with the relative velocity but in the opposing direction
            if(fLocalSpeed > Vector.TOL)
            {
                vLocalVelocity.normalize();
                vDragVector = vLocalVelocity.conjugationVector;
                
                // Determine the resultant force on the element.
                var f:Number;
                if((Vector.interior(Thrust, vLocalVelocity) / (Thrust.length * vLocalVelocity.length)) > 0)
                {
                    f = 2;
                }
                else
                {
                    f = 1;
                }
                
                tmp = 0.5 * MathConst.RHO * fLocalSpeed * fLocalSpeed * this.projectedArea * f;
                
                // simulate fuselage drag
                vResultant = Vector.multiplication(tmp * this._LINEARDRAGCOEFFICIENT, vDragVector);
                
                // Keep a running total of these resultant forces (total force)
                Fb = Vector.addition(Fb, vResultant);
                
                // Calculate the moment about the CG of this element's force
                // and keep a running total of these moments (total moment)
                vtmp = Vector.exterior(this.CD, vResultant);
                
                Mb = Vector.addition(Mb, vtmp);
            }
            
            // Calculate the Port & Starboard bow thruster forces:
            // Keep a running total of these resultant forces (total force)
            Fb = Vector.addition(Fb, Vector.multiplication(3, this.PThrust));
            
            // Calculate the moment about the CG of this element's force
            // and keep a running total of these moments (total moment)
            vtmp = Vector.exterior(this.CPT, this.PThrust);
            Mb = Vector.addition(Mb, vtmp);
            
            // Keep a running total of these resultant forces (total force)
            Fb = Vector.addition(Fb, Vector.multiplication(3, this.FThrust));
            
            // Calculate the moment about the CG of this element's force
            // and keep a running total of these moments (total moment)
            vtmp =  Vector.exterior(this.CST, this.FThrust);
            Mb = Vector.addition(Mb, vtmp);
            
            // do other applied forces here
            Fb = Vector.addition(Fb, Fa);
            vtmp = Vector.exterior(this.Pa, Fa);
            Mb = Vector.addition(Mb, vtmp);
            
            // Calculate rotational drag
            if(this.vAngularVelocity.length > MathConst.TOL)
            {
                vtmp.x = 0;
                vtmp.y = 0;
                tmp = 0.5 * MathConst.RHO * this.vAngularVelocity.z * this.vAngularVelocity.z * this.projectedArea);
                if(this.vAngularVelocity.z > 0) vtmp.z = -( this._ANGULARDRAGCOEFFICIENT * tmp);
                else vtmp.z = this._ANGULARDRAGCOEFFICIENT * tmp;
                
                Mb = Vector.addition(Mb, vtmp);
            }
            // Now add the propulsion thrust
            Fb = Vector.addition(Fb, Thrust);
            
            // Convert forces from model space to earth space
            this.vForces = this.VRotate2D(this.fOrientation, Fb);
            
            this.vMoment = Vector.addition(this.vMoment, Mb);
        }
        
        /**
         * Update Body-Euler
         */
        public function updateBodyEuler(dt:Number):void
        {
            var a:Vector = new Vector();
            var dv:Vector = new Vector();
            var ds:Vector = new Vector();
            var aa:Number = 0;
            var dav:Number = 0;
            var dr:Number = 0;
            
            // Calculate forces and moments:
            this.calcLoad();
            
            // Integrate linear equation of motion:
            a = Vector.division(this.vForces, this.fMass);
            
            dv = Vector.multiplication(dt, a);
            this.vVelocity = Vector.addition(this.vVelocity, dv);
            
            ds = Vector.multiplication(dt, this.vVelocity);
            this.vPosition = Vector.addition(this.vPosition, ds);
            
            // Integrate angular equation of motion:
            aa = this.vMoment.z / this.fInertia;
            
            dav = aa * dt;
            this.vAngularVelocity.z += dav;
            
            dr = MathUtils.RadiansToDegrees(dt * this.vAngularVelocity.z);
            this.fOrientation += dr;
            
            // Misc. calculations:
            this.fSpeed = this.vVelocity.length;
            this.vVelocityBody = this.VRotate2D(-this.fOrientation, this.vVelocity);
        }
        
        /**
         * Modulate the Thrust  
         */
        public function modulateThrust(up:Boolean):void
        {
            var dT:Number = up ? this._DTHRUST : -this._DTHRUST;
            
            this.thrustForce += dT;
            
            if(this.thrustForce > this._MAXTHRUST) this.thrustForce = this._MAXTHRUST;
            if(this.thrustForce < this._MINTHRUST) this.thrustForce = this._MINTHRUST;
        }
        
        /**
         * Set orientation of Thrusters
         */
        public function setThrusters(p:Boolean, s:Boolean):void
        {
            this.PThrust.x = this.PThrust.y = 0;
            this.SThrust.x = this.SThrust.y = 0;
            
            if(p) this.PThrust.x = - this._STEERINGFORCE;
            if(s) this.SThrust.x = - this._STEERINGFORCE;    
        }
        
        /**
         * Convert forces from model space to earth space  
         */
        public function VRotate2D(angle:Number, u:Vector):Vector
        {
            var x:Number, y:Number;
            
            x = u.x * Math.cos(MathUtils.DegreesToRadians(-angle)) + u.y * Math.sin(MathUtils.DegreesToRadians(-angle));
            y = -u.x * Math.sin(MathUtils.DegreesToRadians(-angle)) + u.y * Math.cos(MathUtils.DegreesToRadians(-angle));
            
            return new Vector(x,y,0);
        }
        
    }
}